#include "Chassis_Task.h"
#include "cmsis_os.h"
#include "Balance.h"
#include "pstwo.h"

void Chassis_Task_Entry(void *argument)
{
	
  for(;;)
  {
		if(PS2_RX > 120 || PS2_RX < -120){
			Drive_Motor_IK(0 , 0 , PS2_RX);
		}
		else if(PS2_RY > 120 || PS2_RY < -120){
		  Drive_Motor_IK(PS2_RY , 0 , 0);
		}
		else if((PS2_RX > 120 || PS2_RX < -120) && (PS2_RY > 120 || PS2_RY < -120)){
		  Drive_Motor_IK(PS2_RY , 0 , PS2_RX);
		}
		else if((PS2_RY < 120 || PS2_RY > -120) && (PS2_RX < 120 || PS2_RX > -120)){
		  Drive_Motor_IK(0 , 0 , 0);
		}
		
    osDelay(10);
  }
 
}

